/*
 * ControlLoop.cpp
 *
 *  Created on: Aug 6, 2009
 *      Author: naveenoid
 */

#include "MonitoringLoop.hpp"
#include <ecl/time.hpp>
#include <stdio.h>
#include "./../Common/Common.hpp"

using Octopus::Common::sWait;
using Octopus::Common::uWait;

using Octopus::BoardSetup::TitechBoard;
using ECL::Time::StopWatch;
using ECL::Time::TimeStamp;
using Octopus::ToolKit::Log::Logger;

//using ECL::Time::

namespace Octopus {

namespace MainLoop {

//ControlLoop * ControlLoop::controlLoop;


Loop * MonitoringLoop::getInstance(Logger *log) {
	if(loop == NULL)
	{
		loop = new MonitoringLoop(log);
	}
	return(loop);
}

void MonitoringLoop::remove() {
	printf("Removing Monitoring Loop\n");
	while(!loopFinished)
	{
		uWait(1000);
		printf("waiting for thread to finish\n");
	}

	delete(this);
}

void MonitoringLoop::start() {

//	int ctr = 1;
	StopWatch stopwatch;
	TimeStamp time;

	long int loopTime = (long int)LOOP_TIME;
	long int time1 = 0, time2 = 0, time3 = 0, timeDiff = 0, time1Prev = 0;

//	long int totTime = 0;
	double avgTime;

	int lMeasDigital=0;
//	int pulseOut= 0;

	double fMeasAnalog = 0.0;

	while(true)
	{
		mutex.lock();
		if(!continueLoop)
		{
			printf("Loop trying to stop\n");
			mutex.unlock();
			break;
		}
		mutex.unlock();

		time = stopwatch.elapsed();
		time1 = time.usec();
//		printf("Initial = %ld \t",time1);
//		wait(0.10);

//		uWait(50000);
// Task

//		vector<int> temp(MAX_NUM_OF_SENSOR,0);
		vector<int> temp = titechBoard->readAD();
//		printf("getting each sensor from vector  ");
		for(int i=0; i<MAX_NUM_OF_MOTOR && i< (int)temp.size();i++)
//		for(int i=0; i<temp.size();i++)
		{
			lMeasDigital = temp.at(i);

//			fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * ((1023 - (double)lMeasDigital)/1023);
			fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * (((double)lMeasDigital)/1023);
//			double thetaTemp = ((double)theta/1023);
//			pulseOut = (int)(1100 + 800*((double)theta/1023));
//			printf("Sensor %d, MotorOut : %d", theta, pulseOut);

			printf("S: %d, F: %f; ", i, fMeasAnalog);
			logger->writeValues(lMeasDigital,fMeasAnalog);

//			titechBoard->writeMotor(i,pulseOut);

//			printf("Sensor %d : %d ", i+1, temp.at(i+1));
		}
		printf("\n");



		time = stopwatch.elapsed();
		time2 = time.usec();
//		printf("Final = %ld \t",time2);

		timeDiff = loopTime-(time2-time1);
//		printf("Must wait for %ld uSec\n", timeDiff);

		uWait(timeDiff);

		time = stopwatch.elapsed();

		time3 = time.usec();

		// Job
//		totTime += time3 - time1;
//		avgTime = (double)totTime / (double)ctr;
//		ctr++;

//		printf ("Time3 is %ld uSec and total loopTime is %ld uSec and Average LoopTime is %f\n", time3, totTime, avgTime);

		time1Prev = time1;

		stopwatch.restart();

	}
	loopFinished = true;
	printf ("Average LoopTime was %f\n", avgTime);
	printf("MonitoringLoop thread closing\n");
}

bool MonitoringLoop::stop()
{
	printf("Trying to stop Loop");
	mutex.lock();
	continueLoop = false;
	mutex.unlock();
	return(continueLoop);
}
bool MonitoringLoop::finished()
{
	return(loopFinished);
}

bool MonitoringLoop::isRunning()
{
	return(continueLoop);
}

bool MonitoringLoop::changeSetPoints(vector<double>)
{
	return(true);
}

MonitoringLoop::MonitoringLoop(Logger *log) : sensorValues(MAX_NUM_OF_SENSOR,0), setPoints(MAX_NUM_OF_MOTOR,0) {
	logger = log;
	titechBoard = TitechBoard::getInstance();
	continueLoop = true;
	loopFinished = false;
	printf("Created New MonitoringLoop\n");
}

MonitoringLoop::~MonitoringLoop() {
	titechBoard->remove();
}

}

}
